﻿#region Using Statements
using System;
using System.Collections.Generic;
using System.Text;
using Microsoft.Xna.Framework;
using starLiGHT;

#endregion

#region COPYRIGHT

/*
    Copyright (c) 2008, 2009, 2010 
        Roland Rosenkranz (Glatzemann@email.de)
*/

#endregion

#region LICENSE

/*
    This file is part of starLiGHT.Collision.

    starLiGHT.Collision is free software: you can redistribute it and/or modify
    it under the terms of the GNU Lesser General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    (at your option) any later version.

    starLiGHT.Collision is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU Lesser General Public License for more details.

    You should have received a copy of the GNU Lesser General Public License
    along with starLiGHT.Collision.  If not, see <http://www.gnu.org/licenses/>.

    
    ADDITIONAL (commercial) LICENSES for starLiGHT.Collision are available on request.
*/

#endregion

#region Box2D Copyright
/*
* Copyright (c) 2006-2009 Erin Catto http://www.gphysics.com
*
* This software is provided 'as-is', without any express or implied
* warranty.  In no event will the authors be held liable for any damages
* arising from the use of this software.
* Permission is granted to anyone to use this software for any purpose,
* including commercial applications, and to alter it and redistribute it
* freely, subject to the following restrictions:
* 1. The origin of this software must not be misrepresented; you must not
* claim that you wrote the original software. If you use this software
* in a product, an acknowledgment in the product documentation would be
* appreciated but is not required.
* 2. Altered source versions must be plainly marked as such, and must not be
* misrepresented as being the original software.
* 3. This notice may not be removed or altered from any source distribution.
*/
#endregion

#region Version Stuff
// **************[ starLiGHT.Engine SVN ]**********************
// * $Rev:: 1486                  $:  Revision of last commit *
// * $Author:: glatzemann         $:  Author of last commit   *
// * $Date:: 2010-10-08 15:55:47 #$:  Date of last commit     *
// ************************************************************

// **************[ Box2D-Repository Info ]*********************
// Header-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/WeldJoint.h
// Revision   : r28
// Change-Date: 2009-11-23
//
// Source-File: http://code.google.com/p/box2d/source/browse/trunk/Box2D/Box2D/Dynamics/Joints/WeldJoint.cpp
// Revision   : r28
// Change-Date: 2009-11-23
//
// Status     : Finished!
#endregion

namespace starLiGHT.Physic.TwoD
{
    /// <summary>
    /// Weld joint definition. You need to specify local anchor points 
    /// where they are attached and the relative body angle. The position 
    /// of the anchor points is important for computing the reaction torque.
    /// </summary>
    public class WeldJointDef : JointDef
    {
        public WeldJointDef()
        {
            type = JointType.WELD;
        }

        /// <summary>Initialize the bodies, anchors, and reference angle using the world anchor point.</summary>
        public void Initialize(Body b1, Body b2, Vector2 anchor)
        {
            body1 = b1;
            body2 = b2;
            localAnchor1 = body1.GetLocalPoint(anchor);
            localAnchor2 = body2.GetLocalPoint(anchor);
            referenceAngle = body2.Angle - body1.Angle;
        }

        /// <summary>The local anchor point relative to body1's origin.</summary>
        public Vector2 localAnchor1;

        /// <summary>The local anchor point relative to body2's origin.</summary>
        public Vector2 localAnchor2;

        /// <summary>The body2 angle minus body1 angle in the reference state (radians).</summary>
        public float referenceAngle;
    };

    /// <summary>
    /// A line joint. This joint provides one degree of freedom: translation 
    /// along an axis fixed in body1. You can use a joint limit to restrict 
    /// the range of motion and a joint motor to drive the motion or to 
    /// model joint friction. 
    /// </summary>
    public class WeldJoint : Joint
    {
        public override Vector2 GetAnchorA()
        {
            return m_body1.GetWorldPoint(ref m_localAnchorA);   
        }

        public override Vector2 GetAnchorB()
        {
            return m_body2.GetWorldPoint(ref m_localAnchorB);
        }

        public override Vector2 GetReactionForce(float inv_dt)
        {
            Vector2 P = new Vector2(m_impulse.X, m_impulse.Y);
            return inv_dt * P;
        }

        public override float GetReactionTorque(float inv_dt)
        {
            return inv_dt * m_impulse.Z;
        }

        public WeldJoint(WeldJointDef def)
            : base(def)
        {
            m_localAnchorA = def.localAnchor1;
            m_localAnchorB = def.localAnchor2;
            m_referenceAngle = def.referenceAngle;

            m_impulse = Vector3.Zero;
        }

        internal override void InitVelocityConstraints(TimeStep step)
        {
            Body bA = m_body1;
            Body bB = m_body2;

            // Compute the effective mass matrix.
            Vector2 rA = Math.Mul(bA.Transform.R, m_localAnchorA - bA.LocalCenter);
            Vector2 rB = Math.Mul(bB.Transform.R, m_localAnchorB - bB.LocalCenter);

            // J = [-I -r1_skew I r2_skew] 
            //     [ 0       -1 0       1] 
            // r_skew = [-ry; rx] 

            // Matlab 
            // K = [ mA+r1y^2*iA+mB+r2y^2*iB,  -r1y*iA*r1x-r2y*iB*r2x,          -r1y*iA-r2y*iB] 
            //     [  -r1y*iA*r1x-r2y*iB*r2x, mA+r1x^2*iA+mB+r2x^2*iB,           r1x*iA+r2x*iB] 
            //     [          -r1y*iA-r2y*iB,           r1x*iA+r2x*iB,                   iA+iB] 

            float mA = bA.m_invMass, mB = bB.m_invMass; 
            float iA = bA.m_invI, iB = bB.m_invI; 

            m_mass.col1.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; 
            m_mass.col2.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; 
            m_mass.col3.X = -rA.Y * iA - rB.Y * iB; 
            m_mass.col1.Y = m_mass.col2.X; 
            m_mass.col2.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; 
            m_mass.col3.Y = rA.X * iA + rB.X * iB; 
            m_mass.col1.Z = m_mass.col3.X; 
            m_mass.col2.Z = m_mass.col3.Y; 
            m_mass.col3.Z = iA + iB; 

            if (step.warmStarting) 
            { 
                // Scale impulses to support a variable time step. 
                m_impulse *= step.dtRatio; 

                Vector2 P = new Vector2(m_impulse.X, m_impulse.Y); 

                bA.m_linearVelocity -= mA * P; 
                bA.m_angularVelocity -= iA * (Vector2Extensions.Cross(rA, P) + m_impulse.Z); 

                bB.m_linearVelocity += mB * P; 
                bB.m_angularVelocity += iB * (Vector2Extensions.Cross(rB, P) + m_impulse.Z); 
            } 
            else 
            {
                m_impulse = Vector3.Zero; 
            } 
        }

        internal override void SolveVelocityConstraints(TimeStep step)
        {
            //B2_NOT_USED(step); 

            Body bA = m_body1; 
            Body bB = m_body2; 

            Vector2 vA = bA.m_linearVelocity; 
            float wA = bA.m_angularVelocity; 
            Vector2 vB = bB.m_linearVelocity; 
            float wB = bB.m_angularVelocity; 

            float mA = bA.m_invMass, mB = bB.m_invMass; 
            float iA = bA.m_invI, iB = bB.m_invI; 

            Vector2 rA = Math.Mul(bA.Transform.R, m_localAnchorA - bA.LocalCenter); 
            Vector2 rB = Math.Mul(bB.Transform.R, m_localAnchorB - bB.LocalCenter); 

            // Solve point-to-point constraint 
            Vector2 Cdot1 = vB + Vector2Extensions.Cross(wB, rB) - vA - Vector2Extensions.Cross(wA, rA); 
            float Cdot2 = wB - wA; 
            Vector3 Cdot = new Vector3(Cdot1.X, Cdot1.Y, Cdot2); 

            Vector3 impulse = m_mass.Solve33(-Cdot); 
            m_impulse += impulse; 

            Vector2 P = new Vector2(impulse.X, impulse.Y); 

            vA -= mA * P; 
            wA -= iA * (Vector2Extensions.Cross(rA, P) + impulse.Z); 

            vB += mB * P; 
            wB += iB * (Vector2Extensions.Cross(rB, P) + impulse.Z); 

            bA.m_linearVelocity = vA; 
            bA.m_angularVelocity = wA; 
            bB.m_linearVelocity = vB; 
            bB.m_angularVelocity = wB; 
         }

        internal override bool SolvePositionConstraints(float baumgarte)
        {
            //B2_NOT_USED(baumgarte); 

            Body bA = m_body1;
            Body bB = m_body2;

            float mA = bA.m_invMass, mB = bB.m_invMass; 
            float iA = bA.m_invI, iB = bB.m_invI; 

            Vector2 rA = Math.Mul(bA.Transform.R, m_localAnchorA - bA.LocalCenter); 
            Vector2 rB = Math.Mul(bB.Transform.R, m_localAnchorB - bB.LocalCenter); 

            Vector2 C1 =  bB.m_sweep.c + rB - bA.m_sweep.c - rA; 
            float C2 = bB.m_sweep.a - bA.m_sweep.a - m_referenceAngle; 

            // Handle large detachment. 
            float k_allowedStretch = 10.0f * PhysicSettings.LinearSlop; 
            float positionError = C1.Length(); 
            float angularError = System.Math.Abs(C2); 
            if (positionError > k_allowedStretch) 
            { 
                 iA *= 1.0f; 
                 iB *= 1.0f; 
            } 

            m_mass.col1.X = mA + mB + rA.Y * rA.Y * iA + rB.Y * rB.Y * iB; 
            m_mass.col2.X = -rA.Y * rA.X * iA - rB.Y * rB.X * iB; 
            m_mass.col3.X = -rA.Y * iA - rB.Y * iB; 
            m_mass.col1.Y = m_mass.col2.X; 
            m_mass.col2.Y = mA + mB + rA.X * rA.X * iA + rB.X * rB.X * iB; 
            m_mass.col3.Y = rA.X * iA + rB.X * iB; 
            m_mass.col1.Z = m_mass.col3.X; 
            m_mass.col2.Z = m_mass.col3.Y; 
            m_mass.col3.Z = iA + iB; 

            Vector3 C = new Vector3(C1.X, C1.Y, C2); 

            Vector3 impulse = m_mass.Solve33(-C); 

            Vector2 P = new Vector2(impulse.X, impulse.Y); 

            bA.m_sweep.c -= mA * P;
            bA.m_sweep.a -= iA * (Vector2Extensions.Cross(rA, P) + impulse.Z); 

            bB.m_sweep.c += mB * P;
            bB.m_sweep.a += iB * (Vector2Extensions.Cross(rB, P) + impulse.Z); 

            bA.SynchronizeTransform(); 
            bB.SynchronizeTransform(); 

            return positionError <= PhysicSettings.LinearSlop && angularError <= PhysicSettings.AngularSlop; 
        }

        internal Vector2 m_localAnchorA;
        internal Vector2 m_localAnchorB;
        internal float m_referenceAngle;

        internal Vector3 m_impulse;

        internal Mat33 m_mass;
    }
}
